#include <math.h>
#include <stdio.h>

#include "gps_simulator.h"

signed long latitude_fixed = 0;   //lat*1e7
signed long longitude_fixed = 0;  //long*1e7
signed long height_fixed = 0;       //height in mm  height*1e3 


signed long mm_per_second = 0;
signed int heading_direction = 0;
signed int pitch_direction = 0;


signed int message_per_second = 1;


void set_message_per_second(unsigned long new_value)
{
	message_per_second = new_value;
}

void set_mm_per_second(unsigned long new_value)
{
	mm_per_second = new_value;
}

unsigned long get_mm_per_second(void)
{
	return mm_per_second;
}

void set_heading_direction(unsigned int new_value)
{
	heading_direction = new_value;
}

unsigned int get_heading_direction(void)
{
	return heading_direction;
}

void set_pitch_direction( int new_value)
{
	pitch_direction = new_value;
}

unsigned int get_pitch_direction(void)
{
	return pitch_direction;
}

void set_latitude(unsigned long new_value)
{
	latitude_fixed = new_value;
}

unsigned long get_latitude(void)
{
	return latitude_fixed;
}

void set_latitude_as_float(double new_value)
{
	latitude_fixed = new_value*1e7;
}

double get_latitude_as_float(void)
{
	return latitude_fixed*1e-7;
}

void set_longitude(unsigned long new_value)
{
	longitude_fixed = new_value;
}

unsigned long get_longitude(void)
{
	return longitude_fixed;
}

void set_longitude_as_float(double new_value)
{
	longitude_fixed = new_value*1e7;
}

double get_longitude_as_float(void)
{
	return longitude_fixed*1e-7;
}


void set_height(unsigned long new_value)
{
	height_fixed = new_value;
}

unsigned long get_height(void)
{
	return height_fixed;
}

void set_height_as_float(double new_value)
{
	height_fixed = new_value*1e3;
}

double get_height_as_float(void)
{
	return height_fixed*1e-3;
}


void run_gps_simulator_loop(void)
{
	unsigned long fixed_latitude_sec = (((mm_per_second/message_per_second) * 1e7 ) / MILIMETERS_PER_LATITUDE)* cos(pitch_direction/DEGREES_PER_RADIAN) * cos(heading_direction/DEGREES_PER_RADIAN) ;
	unsigned long fixed_longitude_sec = (((mm_per_second/message_per_second) * 1e7 ) / MILIMETERS_PER_LONGITUDE)* cos(pitch_direction/DEGREES_PER_RADIAN)* sin(heading_direction/DEGREES_PER_RADIAN);
	long mm_height_sec = ((mm_per_second/message_per_second)) * sin(pitch_direction/DEGREES_PER_RADIAN);
	latitude_fixed += fixed_latitude_sec;
	longitude_fixed += fixed_longitude_sec;
	height_fixed += mm_height_sec;


	FILE *file;
	file = fopen("gps.txt","a+"); /* apend file (add text to
					 a file or create a file if it does not exist.*/
	fprintf(file,"%lf,%lf,%lf\n",latitude_fixed*1e-7,longitude_fixed*1e-7,height_fixed*1e-3); /*writes*/
	fclose(file); /*done!*/


}
